#include <memory>
#include <thread>

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "voice_planner/action/voice_planner.hpp"
#include "my_custom_message/msg/my_custom_message.hpp"

class VoicePlannerActionServer : public rclcpp::Node
{
public:
  using VoicePlanner = voice_planner::action::VoicePlanner;
  using GoalHandleVoicePlanner = rclcpp_action::ServerGoalHandle<VoicePlanner>;

  explicit VoicePlannerActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
  : Node("voice_planner_action_server", options)
  {
    using namespace std::placeholders;

    this->action_server_ = rclcpp_action::create_server<VoicePlanner>(
      this,
      "voice_planner",
      std::bind(&VoicePlannerActionServer::handle_goal, this, _1, _2),
      std::bind(&VoicePlannerActionServer::handle_cancel, this, _1),
      std::bind(&VoicePlannerActionServer::handle_accepted, this, _1));

    publisher_ = this->create_publisher<my_custom_message::msg::MyCustomMessage>(
      "/voice_command", 10);

    RCLCPP_INFO(this->get_logger(), "Voice Planner Action Server 已启动");
  }

private:
  rclcpp_action::Server<VoicePlanner>::SharedPtr action_server_;
  rclcpp::Publisher<my_custom_message::msg::MyCustomMessage>::SharedPtr publisher_;

  rclcpp_action::GoalResponse handle_goal(
    const rclcpp_action::GoalUUID & uuid,
    std::shared_ptr<const VoicePlanner::Goal> goal)
  {
    RCLCPP_INFO(this->get_logger(), "收到目标请求，type=%d", goal->type);
    (void)uuid;
    
    // 验证type的有效性
    if (goal->type < 1 || goal->type > 6) {
      RCLCPP_WARN(this->get_logger(), "无效的type值: %d，应该在1-6之间", goal->type);
      return rclcpp_action::GoalResponse::REJECT;
    }

    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
  }

  rclcpp_action::CancelResponse handle_cancel(
    const std::shared_ptr<GoalHandleVoicePlanner> goal_handle)
  {
    RCLCPP_INFO(this->get_logger(), "收到取消请求");
    (void)goal_handle;
    return rclcpp_action::CancelResponse::ACCEPT;
  }

  void handle_accepted(const std::shared_ptr<GoalHandleVoicePlanner> goal_handle)
  {
    using namespace std::placeholders;
    // 在新线程中执行，以避免阻塞主线程
    std::thread{std::bind(&VoicePlannerActionServer::execute, this, _1), goal_handle}.detach();
  }

  void execute(const std::shared_ptr<GoalHandleVoicePlanner> goal_handle)
  {
    RCLCPP_INFO(this->get_logger(), "开始执行目标");
    
    const auto goal = goal_handle->get_goal();
    auto feedback = std::make_shared<VoicePlanner::Feedback>();
    auto result = std::make_shared<VoicePlanner::Result>();

    // 创建消息
    auto message = my_custom_message::msg::MyCustomMessage();
    
    // 初始化所有字段为0
    message.type = 0;
    message.num1 = 0;
    message.num2 = 0;
    message.num3 = 0;
    message.num4 = 0;
    message.num5 = 0;
    message.num6 = 0;
    message.num7 = 0;
    message.num8 = 0;
    message.num9 = 0;
    message.num10 = 0;
    message.num11 = 0;
    message.num12 = 0;
    message.num13 = 0;
    message.num14 = 0;
    message.num15 = 0;
    message.num16 = 0;

    // 设置type
    message.type = goal->type;

    // 根据type设置不同的值
    switch (goal->type) {
      case 1:
        // type=1，其余数字为0（已经初始化为0）
        feedback->status = "处理type=1，所有数字字段设置为0";
        RCLCPP_INFO(this->get_logger(), "%s", feedback->status.c_str());
        goal_handle->publish_feedback(feedback);
        break;

      case 2:
        // type=2，num1为数组的第一个元素
        if (goal->data.size() >= 1) {
          message.num1 = goal->data[0];
          feedback->status = "处理type=2，num1设置为数组第一个元素";
        } else {
          feedback->status = "处理type=2，但数组为空";
          RCLCPP_WARN(this->get_logger(), "数组为空");
        }
        RCLCPP_INFO(this->get_logger(), "%s", feedback->status.c_str());
        goal_handle->publish_feedback(feedback);
        break;

      case 3:
        // type=3，num1~8对应数组的前8个元素
        feedback->status = "处理type=3，num1~8设置为数组前8个元素";
        RCLCPP_INFO(this->get_logger(), "%s", feedback->status.c_str());
        goal_handle->publish_feedback(feedback);
        
        if (goal->data.size() >= 1) message.num1 = goal->data[0];
        if (goal->data.size() >= 2) message.num2 = goal->data[1];
        if (goal->data.size() >= 3) message.num3 = goal->data[2];
        if (goal->data.size() >= 4) message.num4 = goal->data[3];
        if (goal->data.size() >= 5) message.num5 = goal->data[4];
        if (goal->data.size() >= 6) message.num6 = goal->data[5];
        if (goal->data.size() >= 7) message.num7 = goal->data[6];
        if (goal->data.size() >= 8) message.num8 = goal->data[7];
        break;

      case 4:
        // type=4，num1为数组的第一个元素
        if (goal->data.size() >= 1) {
          message.num1 = goal->data[0];
          feedback->status = "处理type=4，num1设置为数组第一个元素";
        } else {
          feedback->status = "处理type=4，但数组为空";
          RCLCPP_WARN(this->get_logger(), "数组为空");
        }
        RCLCPP_INFO(this->get_logger(), "%s", feedback->status.c_str());
        goal_handle->publish_feedback(feedback);
        break;

      case 5:
        // type=5，num1~16对应数组的前16个元素
        feedback->status = "处理type=5，num1~16设置为数组的前16个元素";
        RCLCPP_INFO(this->get_logger(), "%s", feedback->status.c_str());
        goal_handle->publish_feedback(feedback);
        
        if (goal->data.size() >= 1) message.num1 = goal->data[0];
        if (goal->data.size() >= 2) message.num2 = goal->data[1];
        if (goal->data.size() >= 3) message.num3 = goal->data[2];
        if (goal->data.size() >= 4) message.num4 = goal->data[3];
        if (goal->data.size() >= 5) message.num5 = goal->data[4];
        if (goal->data.size() >= 6) message.num6 = goal->data[5];
        if (goal->data.size() >= 7) message.num7 = goal->data[6];
        if (goal->data.size() >= 8) message.num8 = goal->data[7];
        if (goal->data.size() >= 9) message.num9 = goal->data[8];
        if (goal->data.size() >= 10) message.num10 = goal->data[9];
        if (goal->data.size() >= 11) message.num11 = goal->data[10];
        if (goal->data.size() >= 12) message.num12 = goal->data[11];
        if (goal->data.size() >= 13) message.num13 = goal->data[12];
        if (goal->data.size() >= 14) message.num14 = goal->data[13];
        if (goal->data.size() >= 15) message.num15 = goal->data[14];
        if (goal->data.size() >= 16) message.num16 = goal->data[15];
        break;

      case 6:
        // type=6，num1为数组的第一个元素
        if (goal->data.size() >= 1) {
          message.num1 = goal->data[0];
          feedback->status = "处理type=6，num1设置为数组第一个元素";
        } else {
          feedback->status = "处理type=6，但数组为空";
          RCLCPP_WARN(this->get_logger(), "数组为空");
        }
        RCLCPP_INFO(this->get_logger(), "%s", feedback->status.c_str());
        goal_handle->publish_feedback(feedback);
        break;

      default:
        result->success = false;
        result->message = "无效的type值";
        goal_handle->succeed(result);
        return;
    }

    // 发布消息
    publisher_->publish(message);
    RCLCPP_INFO(this->get_logger(), "已发布消息，type=%d", message.type);

    // 检查是否已被取消
    if (goal_handle->is_canceling()) {
      result->success = false;
      result->message = "目标已被取消";
      goal_handle->canceled(result);
      RCLCPP_INFO(this->get_logger(), "目标已取消");
      return;
    }

    // 成功完成
    result->success = true;
    result->message = "成功发布消息";
    goal_handle->succeed(result);
    RCLCPP_INFO(this->get_logger(), "目标执行成功");
  }
};

int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);

  auto action_server = std::make_shared<VoicePlannerActionServer>();

  rclcpp::executors::MultiThreadedExecutor executor;
  executor.add_node(action_server);
  executor.spin();

  rclcpp::shutdown();
  return 0;
}

